from pb.ros.geometry_msgs import Pose_pb2
from pb.ros.geometry_msgs import PoseStamped_pb2
from pb.ros.nav_msgs import Odometry_pb2
from pb.ros.visualization_msgs import Marker_pb2
from pb.ros.geometry_msgs import Point_pb2
import proxy

import os
import time
import math
import threading
import sys

node = proxy.Connector('tcp://172.22.56.12:15555') #在709的电脑上使用该IP
#node = proxy.Connector('tcp://12.12.12.204:15555') #在集群的电脑上使用该IP
publish_cmd_pos = node.Advertise('FK','M300_0', '/FK/M300_0/cmd_pos', Pose_pb2.Pose) #发给动力学模型的控制输入

x1,y1,z1,x,y,z,w = 0,0,0,0,0,0,0
def callback_dp(pos):
    global x1
    global y1
    global z1
    global x
    global y
    global z
    global w
    x1 = pos.position.x  
    y1 = pos.position.y
    z1 = pos.position.z
    x = pos.orientation.x
    y = pos.orientation.y
    z = pos.orientation.z
    w = pos.orientation.w
    #print(x1)

subcribe_dp = node.Subscribe('FK','M300_0', '/FK/M300_0/desired_pos', Pose_pb2.Pose,callback_dp)#规划算法的输出

def sub_dp():
	while 1:
		subcribe_dp.SpinOnce()


class Publish:
    def cmd_pos(self):
        cmd_pos = Pose_pb2.Pose()
        cmd_pos.position.x = x1
        cmd_pos.position.y = y1
        cmd_pos.position.z = z1
        cmd_pos.orientation.x = x
        cmd_pos.orientation.y = y
        cmd_pos.orientation.z = z
        cmd_pos.orientation.w = w
        #print(cmd_pos.position.x)
        publish_cmd_pos.Publish(cmd_pos)

    def pub(self):
        while 1:
            self.cmd_pos()


def main(args):
    
    publish = Publish()
    thread_pub = threading.Thread(target = publish.pub)
    thread_pub.start()
    
    thread_sub_dp = threading.Thread(target = sub_dp)
    thread_sub_dp.start()

    n = input('')


if __name__ == '__main__':
    main(sys.argv)
